#pragma once

#include <rclcpp/rclcpp.hpp>
#include <inttypes.h>
#include <memory>

#include <tf2_ros/transform_broadcaster.h>
#include <iostream>
#include <unistd.h>
#include <serial/serial.h> //ROS的串口包 http://wjwwood.io/serial/doc/1.1.0/index.html
#include <math.h>
#include <fstream>

#include <sensor_msgs/msg/imu.hpp>
#include <geometry_msgs/msg/pose_with_covariance.hpp>

#include <string>
#include <array>

#include <ament_index_cpp/get_package_prefix.hpp>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <Eigen/Core>

#include "imu_driver/crc_table.h"
#include "imu_driver/fdilink_data_struct.h"


// 协方差配置
namespace robot_covariance_config
{
    // IMU姿态协方差配置
    inline constexpr std::array<double, 36> imu_pose_covariance = {
        1e-3, 0,    0,    0,    0,    0,
        0,    1e-3, 0,    0,    0,    0,
        0,    0,    1e6,  0,    0,    0,
        0,    0,    0,    1e6,  0,    0,
        0,    0,    0,    0,    1e6,  0,
        0,    0,    0,    0,    0,    1e3
    };
}


class ImuDriver : public rclcpp::Node{
    public:
        ImuDriver();
        ~ImuDriver();

        void InitParameters();
        void InitPublishers();
        void InitSerial();
        void InitFrameHandlers();

        void ProcessLoop();

        bool ReadFrameHeader(FDILink::fdilink_header& header);
        void ProcessSpecialFrame(const FDILink::fdilink_header& header);
        bool ValidateDataLength(const FDILink::fdilink_header& header);
        bool ValidateHeaderCRC8(const FDILink::fdilink_header& header); 
        void ProcessSerialNumber(const FDILink::fdilink_header &header);
        bool ReadFramePayload(const FDILink::fdilink_header &header, std::vector<uint8_t> &payload);

        void HandleImuFrame();
        void HandleAhrsFrame();
        void HandleInsGpsFrame();
        void HandleGeoFrame();
       
        void PrintDebugHeader(const FDILink::fdilink_header& h); 
    private:
        // 调试模式
        bool __if_debug;

        //sum info 序号统计
        int __sn_lost = 0;      // 是统计丢失的帧数量
        int __crc_error = 0;    //是统计校验错误的帧数量
        uint8_t __read_sn = 0;  //是当前已接收的最新序列号（SN
        bool __frist_sn;        //是是否为首次接收序列号

        //设备类型
        int __device_type = 1;

        //serial
        serial::Serial __serial; //声明串口对象
        std::string __port; //串口号
        int __baudrate; //波特率
        int __serial_timeout; //串口超时时间

        // buffers
        FDILink::imu_frame_read __imu_frame;
        FDILink::ahrs_frame_read __ahrs_frame;
        FDILink::insgps_frame_read __insgps_frame;
        FDILink::Geodetic_Position_frame_read __geo_frame;

        // 定义帧处理函数类型，参数为解析好的帧头，返回bool表示处理成功与否
        // 定义frame_handlers为带header参数的函数类型，方便传递帧头数据
        using FrameHandler = std::function<void(const std::vector<uint8_t>&)>;
        std::unordered_map<uint8_t, FrameHandler> __frame_handlers;

        // CRC helpers // 在crc_table.h中定义

        // topic name
        std::string __imu_topic_name = "/imu",\
        __imu_pose_topic_name = "/imu_pose";

        // frame id
        std::string __imu_frame_id = "imu_link";
        std::string __imu_pose_frame_id = "imu_link";
        
        //Publisher
        rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr  __imu_pub;
        rclcpp::Publisher<geometry_msgs::msg::PoseWithCovariance>::SharedPtr __imu_pos_pub;

};